// MultiStepper
// -*- mode: C++ -*-
//
// Control both Stepper motors at the same time with different speeds
// and accelerations. 
// Requires the AFMotor library (https://github.com/adafruit/Adafruit-Motor-Shield-library)
// And AccelStepper with AFMotor support (https://github.com/adafruit/AccelStepper)
// Public domain!

#include <AccelStepper.h>
#include <AFMotor.h>
//#include <aJSON.h>

struct Motor 
{
  byte state; // 0 - остановлен, 1 - поиск датчика, 2 - идет в заданную позицию 
  boolean stoped;
};

Motor motors[2];

// two stepper motors one on each port
AF_Stepper motor0(200, 1);
AF_Stepper motor1(200, 2);

// you can change these to SINGLE, DOUBLE or INTERLEAVE or MICROSTEP!
// wrappers for the first motor!
void forwardstep0() {  
  motor0.onestep(FORWARD,  MICROSTEP);
}
void backwardstep0() {  
  motor0.onestep(BACKWARD,  MICROSTEP);
}
// wrappers for the second motor!
void forwardstep1() {  
  motor1.onestep(FORWARD,  MICROSTEP);
}
void backwardstep1() {  
  motor1.onestep(BACKWARD,  MICROSTEP);
}

// Motor shield has two motor ports, now we'll wrap them in an AccelStepper object
AccelStepper stepper0(forwardstep0, backwardstep0);
AccelStepper stepper1(forwardstep1, backwardstep1);

long motorPosition[2];
unsigned long time = 0;
unsigned long loopCounter = 0;

void setup()
{  
  stepper0.setMaxSpeed(600.0);
  stepper0.setAcceleration(400.0);

  stepper1.setMaxSpeed(600.0);
  stepper1.setAcceleration(400.0);

  pinMode(14, INPUT);
  pinMode(15, INPUT);

  //stepper2.moveTo(1000000);
  motorPosition[0] = 0;
  motorPosition[1] = 0;

  motors[0].state = 0;
  motors[1].state = 0;

  Serial.begin(9600);
  time = millis();
  //establishContact();  // send a byte to establish contact until receiver responds     
}

int i = 0;
int inChar;
String inString = "";

void sendState()
{

  char s[255] = "s:mp1:";
  long motor0CurrentPosition = stepper0.currentPosition();
  long motor1CurrentPosition = stepper1.currentPosition();
  char s1[11] = "";
  ltoa(motor0CurrentPosition, s1, 10);
  strcat( s, s1 );
  strcat ( s,";mp2:" );
  ltoa(motor1CurrentPosition, s1, 10);
  strcat( s, s1);
  strcat (s,";ss1:");
  itoa(digitalRead(14), s1, 10);
  strcat( s, s1);   
  strcat (s,";ss2:");
  itoa(digitalRead(15), s1, 10);
  strcat( s, s1);      
  strcat (s,";");
  //ltoa(loopCounter, s1, 10);
  //strcat( s, s1);
  Serial.flush();
  Serial.println( s ); 
  // delay(1);
}

void sendSensorSearchResult(int motor, int result, long motorPosition)
{

  char s[255] = "z:m:";
  char s1[11] = "";
  itoa(motor, s1, 10);
  strcat( s, s1);
  strcat (s,";r:");     
  itoa(result, s1, 10);
  strcat( s, s1);
  strcat (s,";p:");  
  ltoa(motorPosition, s1, 10);
  strcat( s, s1 );
  strcat (s,";");
  Serial.flush();
  Serial.println( s ); 
  //delay(1);
}

/* Принимаемые команды
 Поворота установки угла ротора двигателя: A<M><S>, M - номер двигателя (0, 1), S - число шагов (-long .. +long), возвращает слово состояния контроллера после остановки. 
 Сброс счетчика шагов в 0: R<M>, M - номер двигателя (0, 1), возврата нет.
 Поиск датчика: Z<M><S>, M - номер двигателя (0, 1), S - максимальное число шагов для поиска (-long .. +long), возвращает сообщение о результатах поиска 
 z:m:<M>;r:<R>;p:<P>; M - номер двигателя (0, 1), R - результат (0 - датчик не найден, 1 - датчик найден), P - позиция в шагах (-long .. +long).  
 Запрос состояния контроллера: S, возвращает слово состояния контроллера.
 */

void parseCommand()
{
  char s[11] = "";
  // Разбор команды установки угла ротора двигателя в шагах
  if (isAlpha(inString.charAt(0)) && inString.charAt(0) == 'A')
  {
    // Номера двигателей 0 и 1
    if (inString.charAt(1) == '0' || inString.charAt(1) == '1')
    {
      // Определение номера двигателя
      byte motorNumber = (byte)inString.charAt(1) - '0';
      // Выделение числа шагов относительно начала
      inString.substring(2).toCharArray(s, sizeof(s)); 
      motorPosition[motorNumber] = atol( s );
      //Serial.println(s);
    }
  }
  // Сброс счетчика шагов в 0
  else if (isAlpha(inString.charAt(0)) && inString.charAt(0) == 'R')
  {
    byte motorNumber = (byte)inString.charAt(1) - '0';
    if (motorNumber == 0 )
    {
      //Serial.println("Resetted");
      motorPosition[motorNumber] = 0;
      stepper0.setCurrentPosition(0);
      stepper0.setMaxSpeed(800.0);
      stepper0.setAcceleration(600.0);
    }
    else if (motorNumber == 1 )

    {
      motorPosition[motorNumber] = 0;
      stepper1.setCurrentPosition(0);
      stepper1.setMaxSpeed(800.0);
      stepper1.setAcceleration(600.0);
    }
  }
  else if (isAlpha(inString.charAt(0)) && inString.charAt(0) == 'Z')
  {
    // Номера двигателей 0 и 1
    if (inString.charAt(1) == '0' || inString.charAt(1) == '1')
    {
      // Определение номера двигателя
      byte motorNumber = (byte)inString.charAt(1) - '0';
      // Выделение числа шагов
      inString.substring(2).toCharArray(s, sizeof(s));
      motors[ motorNumber].state = 1; 
      if ( motorNumber == 0 )
      {
        motorPosition[motorNumber] = stepper0.currentPosition() - atol( s );
      }
      else if ( motorNumber == 1 )
      {
        motorPosition[motorNumber] = stepper1.currentPosition() - atol( s );
      }      
    }
  }
  else if (isAlpha(inString.charAt(0)) && inString.charAt(0) == 'S')
  {
    sendState();
  }  
}

boolean stringComplete = false;

// Делает разовое перемещение ротора двигателя в цикле контроллера
void moveMotor(byte i)
{

  long motorCurrentPosition;
  if ( i == 0 )
  {
    stepper0.moveTo(motorPosition[i]);
    motorCurrentPosition = stepper0.currentPosition();

    stepper0.run();
  }
  else if ( i == 1 )
  {
    stepper1.moveTo(motorPosition[i]);
    motorCurrentPosition = stepper1.currentPosition();
    stepper1.run();  
  }
  if ( motors[i].state == 2 && motorPosition[i] == motorCurrentPosition )
  {
    motors[i].state = 0;  
    sendState();
  }
  else if (motors[i].state == 0 && motorPosition[i] != motorCurrentPosition)
    motors[i].state =
      2;              
}

// Проверяет, не найден ли датчик
void checkSensorSearch(byte motor)
{
  if ( motor == 0 )
  {
    if ( digitalRead(14) != 0 )
    { 
      motors[motor].state = 0;
      motorPosition[motor] = stepper0.currentPosition();
      sendSensorSearchResult(motor, 1, motorPosition[motor]);
    }
    else if ( stepper0.currentPosition() == motorPosition[motor] )
    {
      motors[motor].state = 0;
      motorPosition[motor] = stepper0.currentPosition();
      sendSensorSearchResult(i, 0, motorPosition[motor]);
    }
  }
  else if ( motor == 1 )
  {
    if ( digitalRead(15) != 0 )
    { 
      motors[motor].state = 0;
      motorPosition[motor] = stepper1.currentPosition();
      sendSensorSearchResult(motor, 1, motorPosition[motor]);
    }
    else if ( stepper1.currentPosition() == motorPosition[motor] )
    {
      motors[motor].state = 0;
      motorPosition[motor] = stepper1.currentPosition();
      sendSensorSearchResult(motor, 0, motorPosition[motor]);
    }
  }
}

void loop()
{
  for (byte i = 0; i <2; i++)
  {    
    moveMotor(i);
    if (motors[i].state == 1)
      checkSensorSearch(i);
  }

  // Read serial input:
  while (Serial.available() > 0 && stringComplete == false) 
  {
    inChar = Serial.read();
    inString += (char)inChar;
    if (inChar == '\n')
    {
      //Serial.print(inString);
      stringComplete = true;
    }
  }
  if (stringComplete)
  {
    parseCommand();
    inString = "";
    stringComplete = false;
    //break;
  }
  /* if (millis() - time > 500)
   {
   sendState();
   time = millis();
   }
   */
  loopCounter++;
}



